# -*- coding: iso-8859-1 -*-
# file Joint.py
# author Rocco Martino
#
############################################################################
#    Copyright (C) 2015 by Rocco Martino                                   #
#    martinorocco@gmail.com                                                #
#                                                                          #
#    This program is free software; you can redistribute it and/or modify  #
#    it under the terms of the GNU Lesser General Public License as        #
#    published by the Free Software Foundation; either version 2.1 of the  #
#    License, or (at your option) any later version.                       #
#                                                                          #
#    This program is distributed in the hope that it will be useful,       #
#    but WITHOUT ANY WARRANTY; without even the implied warranty of        #
#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         #
#    GNU Lesser General Public License for more details.                   #
#                                                                          #
#    You should have received a copy of the GNU Lesser General Public      #
#    License along with this program; if not, write to the                 #
#    Free Software Foundation, Inc.,                                       #
#    59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             #
############################################################################

############################################################################
from . import Transformable, ODEObject

from mathutils import Vector, Matrix, Euler
############################################################################




############################################################################
# ........................................................................ #
# ........................................................................ #
############################################################################




############################################################################
# ........................................................................ #
class Joint(Transformable.Transformable):





############################################################################
    Body1 = None
    UseLocalValues = False
############################################################################









############################################################################
    def __init__(self, obj, data, constraint):
        super(Joint, self).__init__(constraint, data)

        self.ClassName = "osgODE::Joint"

        self.WriteMatrixTransform = False

        self.Body1 = obj

        self.UseObjectName = False

        self.CheckUniqueID = False

        self.Name = constraint.name

        self.UseLocalValues = False
############################################################################




############################################################################
    def writeData(self, writer):
        super(Joint, self).writeData(writer)


        writer.moveIn("Body1 TRUE")

        ODEObject.createObject( self.Body1, self.Data ).writeObject(writer)

        writer.moveOut("Body1 TRUE")



        if self.BlenderObject.target:

            writer.moveIn("Body2 TRUE")

            ODEObject.createObject( self.BlenderObject.target, self.Data ).writeObject(writer)

            writer.moveOut("Body2 TRUE")






        anchor1 = Vector()
        anchor1.x = self.BlenderObject.pivot_x
        anchor1.y = self.BlenderObject.pivot_y
        anchor1.z = self.BlenderObject.pivot_z

        anchor_world_space = self.Body1.matrix_local * anchor1

        if not self.UseLocalValues:
            anchor1 = anchor_world_space
            anchor2 = anchor_world_space

        elif self.BlenderObject.target:
            inv2 = Matrix( self.BlenderObject.target.matrix_local )
            inv2.invert()

            anchor2 = inv2 * anchor_world_space

        else:
            anchor2 = anchor_world_space


        writer.writeLine("Anchor1 %f %f %f" %(anchor1.x, anchor1.y, anchor1.z))
        writer.writeLine("Anchor2 %f %f %f" %(anchor2.x, anchor2.y, anchor2.z))





        rx = self.BlenderObject.axis_x
        ry = self.BlenderObject.axis_y
        rz = self.BlenderObject.axis_z

        m1 = Matrix().to_3x3()
        m2 = Matrix().to_3x3()
        m3 = Matrix().to_3x3()

        m1.rotate( Euler( (-rx, 0.0, 0.0) ) )
        m2.rotate( Euler( (0.0, -ry, 0.0) ) )
        m3.rotate( Euler( (0.0, 0.0, -rz) ) )

        m = m1 * m2 * m3




        if self.UseLocalValues:
            rot_matrix = Matrix().to_3x3()

        else:
            rot_matrix = Matrix(self.Body1.matrix_local).to_3x3()


        axis = Vector()
        axis.x = m[0][0]
        axis.y = m[0][1]
        axis.z = m[0][2]
        axis = rot_matrix * axis
        axis.normalize()

        writer.writeLine("Axis1 %f %f %f" %(axis.x, axis.y, axis.z))



        axis = Vector()
        axis.x = m[1][0]
        axis.y = m[1][1]
        axis.z = m[1][2]
        axis = rot_matrix * axis
        axis.normalize()

        writer.writeLine("Axis2 %f %f %f" %(axis.x, axis.y, axis.z))



        axis = Vector()
        axis.x = m[2][0]
        axis.y = m[2][1]
        axis.z = m[2][2]
        axis = rot_matrix * axis
        axis.normalize()

        writer.writeLine("Axis3 %f %f %f" %(axis.x, axis.y, axis.z))
############################################################################




# ........................................................................ #
############################################################################









############################################################################
# ........................................................................ #
def create( obj, data, constraint ):

    from . import BallJoint, HingeJoint, Generic6DofJoint


    if constraint.pivot_type == "BALL":
        return BallJoint.BallJoint(obj, data, constraint)

    elif constraint.pivot_type == "HINGE":
        return HingeJoint.HingeJoint(obj, data, constraint)

    elif constraint.pivot_type == "GENERIC_6_DOF":
        return Generic6DofJoint.Generic6DofJoint(obj, data, constraint)

    else:
        printf("object %s: unsupported joint type: %s" % (obj.name, constraint.pivot_type))


    return None
# ........................................................................ #
############################################################################
